#include "com.h"
#include "string.h"
#include "foc.h"
#include "global_def.h"
#include "usart.h"
#include "motor_runtime_param.h"
#include <stdio.h>
// 全局接收缓冲区和计数
uint8_t uart_rx_buf[UART_FRAME_LEN] = {0};
uint8_t uart_rx_cnt = 0;
UART_Frame rx_frame;
void motorcontrol(int8_t mode,int16_t param1,int16_t param2){
    switch(mode){
        case MODE_SPEED:
        motor_control_context.speed = param1;       
        motor_control_context.type = control_type_speed; 
        break;
        case MODE_POSITION:
        motor_control_context.position = deg2rad(param1); 
        motor_control_context.type = control_type_position;
        break;
        case MODE_TORQUE:
        motor_control_context.torque_norm_d = 0;
        motor_control_context.torque_norm_q = param1; // 百分比强度
        motor_control_context.type = control_type_torque;
        break;
        case MODE_SPEED_TORQUE:
        motor_control_context.type = control_type_speed_torque;
        motor_control_context.speed = param1;
        motor_control_context.max_torque_norm = param2;
        break;
        case MODE_POSITION_SPEED_TORQUE:
        motor_control_context.type = control_type_position_speed_torque;
        motor_control_context.position = param1;
        motor_control_context.max_speed = param2;
        motor_control_context.max_torque_norm = 0.9;
        break;

            
        


    }
}

/**
 * @brief 解析UART接收的字节流为帧结构
 */


static uint8_t UART_ParseFrame(void) {
  // 校验帧头帧尾
  if (uart_rx_buf[0] != 0xAA || uart_rx_buf[7] != 0x55) {
    return 1; // 帧格式错误
  }

  // 解析数据
  rx_frame.head = uart_rx_buf[0];
  rx_frame.motor_id = uart_rx_buf[1];
  rx_frame.mode = uart_rx_buf[2];
  rx_frame.param1 = (int16_t)((uart_rx_buf[3] << 8) | uart_rx_buf[4]);
  rx_frame.param2 = (int16_t)((uart_rx_buf[5] << 8) | uart_rx_buf[6]);
  rx_frame.tail = uart_rx_buf[7];

  return 0; // 解析成功
}

/**
 * @brief 处理解析后的帧，调用motorcontrol函数
 */
void UART_ProcessFrame(UART_Frame *frame) {
  // 校验电机ID（确保指令是发给自己的）
  if (frame->motor_id == MOTOR1) { // 此处MOTOR1需与当前驱动对应
    motorcontrol(frame->mode, frame->param1, frame->param2);
    
  }
}

/**
 * @brief UART接收中断回调函数（HAL库自动调用）
 * 每接收1字节进入一次，累计接收完整帧后解析
 */
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
  
  if (huart->Instance == USART2) { // 电机1对应USART2
    // 接收1字节，计数+1
    
    uart_rx_cnt++;
  
    
    

    // 接收满一帧后解析
    if (uart_rx_cnt >= UART_FRAME_LEN) {
      if (UART_ParseFrame() == 0) { // 解析成功
        UART_ProcessFrame(&rx_frame);
      }
      // 重置缓冲区和计数，准备接收下一帧
      memset(uart_rx_buf, 0, UART_FRAME_LEN);
      uart_rx_cnt = 0;
    }

    // 继续接收下一字节
    HAL_UART_Receive_IT(huart, &uart_rx_buf[uart_rx_cnt], 1);
  }
}

/**
 * @brief 初始化UART从机接收
 */
void UART_SlaveInit(void) {
  // 启动中断接收（每次接收1字节）
  HAL_UART_Receive_IT(&huart2, &uart_rx_buf[0], 1);
  
}


void debugcom(void){
  printf("%.3f\n",rad2deg(motor_logic_angle));
  //printf("%.3f\n",motor_i_q);
  HAL_Delay(500);
}


